#include"ros/ros.h"
#include"tello_show/Position.h"
#include"tello_show/Series.h"
#include<iostream>
#include "std_msgs/String.h"
#include"stdlib.h"
#include"string.h"

/*
    发布方实现：
        1.包含头文件
            自定义文本类型--->Person.h
        2.初始化ROS节点
            ros::init(argc,argv,"节点名称")
        3.创建节点句柄
        4.创建发布者对象
        5.编写发布逻辑并发布数据
*/
using namespace std;

// double tello2_x, tello2_y, tello6_x,tello6_y;
int count1 = 0,count2 = 0,n1 = 0,n2 = 0;
tello_show::Series pos_cmd1,pos_cmd2;
tello_show::Position p1,p2;

//接收位置序列的回调函数
void doposition1(const tello_show::Series::ConstPtr& msg)
{
    n1 = sizeof(msg);
    pos_cmd1 = *msg;
    count1 = 0;
    p1.position_x = pos_cmd1.pos_series_x[count1];
    p1.position_y = pos_cmd1.pos_series_y[count1];
    count1 ++;
    ROS_INFO("接收到新的位置组，重置计数");
}
void doposition2(const tello_show::Series::ConstPtr& msg)
{
    n2 = sizeof(msg);
    pos_cmd2 = *msg;
    count2 = 0;
    p2.position_x = pos_cmd2.pos_series_x[count2];
    p2.position_y = pos_cmd2.pos_series_y[count2];
    count2 ++;
    ROS_INFO("接收到新的位置组，重置计数");
}

//接收到达信息的回调函数
void nextpos1(const std_msgs::String::ConstPtr& msg)
{
    if(count1 < n1)
    {
        p1.position_x = pos_cmd1.pos_series_x[count1];
        p1.position_y = pos_cmd1.pos_series_y[count1];
        count1 ++;
    }
}
void nextpos2(const std_msgs::String::ConstPtr& msg)
{
    if(count2 < n2)
    {
        p2.position_x = pos_cmd2.pos_series_x[count2];
        p2.position_y = pos_cmd2.pos_series_y[count2];
        count2 ++;
    }
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    //1.初始化ros节点
    ros::init(argc,argv,"position_cmd");

    //2.创建ros句柄
    ros::NodeHandle nh;

    //3.创建发布者对象
    ros::Publisher pub1 = nh.advertise<tello_show::Position>("pos_cmd1",1000);
    ros::Publisher pub2 = nh.advertise<tello_show::Position>("pos_cmd2",1000);

    //订阅pos_series的位置序列
    ros::Subscriber pos_sub1 = nh.subscribe("pos_series1", 10, doposition1);
    ros::Subscriber pos_sub2 = nh.subscribe("pos_series2", 10, doposition2);
 
    //监听是否到达指定目标点
    ros::Subscriber pos_arrive1 = nh.subscribe("arrive1", 10, nextpos1);
    ros::Subscriber pos_arrive2 = nh.subscribe("arrive2", 10, nextpos2);   

    ros::Rate rate(10);
        //发布频率为0.1s一次
    ros::Duration(2.0).sleep();
        //发布前进行2s休眠
    //初始起飞位置的设定
    p1.position_x = 0;
    p1.position_y = 0;
    p2.position_x = 0;
    p2.position_y = 0;

    while(ros::ok())
    {
        pub1.publish(p1);
        pub2.publish(p2);
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}
